#include <GL/glut.h>
#include "TinkerToy.h"
#include <stdio.h>
#include <math.h>
#include "BMPWriter.h"

#define ADDPARTICLES	0
#define ADDCONSTRAINTS	1
#define SIMULATE		2
#define PI				3.1415926
#define RADIUS			30


const int WIN_WIDTH = 800;
const int WIN_HEIGHT = 600;
const double ORTHO_RIGHT = 240;
const double ORTHO_UP = 180;
const double ORTHO_LEFT = 0;
const double ORTHO_BOTTOM = 0;

struct {

	BYTE b;

	BYTE g;

	BYTE r;

} pRGB[480][640]; 

float *result;

int *displayID;

char *filename="../bmp/000.bmp";
char file[15];

bool addc=false;
bool drawc=false;
bool onCircle=false;
bool capture=false;
int start;
int step=ADDPARTICLES;

double pos[2];
double neg[2];

TinkerToy *toy;

void initCircle(double x, double y, int step);

void init(void) 
{
	glClearColor (1.0, 1.0, 1.0, 1.0);
	glShadeModel (GL_FLAT);
	initCircle(0,0,5);
	result=(float*)malloc(640*480*sizeof(float)*3);

	for (int i=0;i<14;i++)
	{
		file[i]=filename[i];
	}
	file[14]=0;

}

void initCircle(double x, double y, int step)
{
	displayID=(int*)malloc(sizeof(int));
	displayID[0]=glGenLists(1);

	glNewList(displayID[0], GL_COMPILE);

	glBegin(GL_LINE_STRIP);

	for (int i=0;i<=360;i+=step)
	{
		double phi=(double)i/180.0*PI;
		double rx=sin(phi)*RADIUS;
		double ry=cos(phi)*RADIUS;		

		glVertex2f(rx,ry);		
	}

	glEnd();

	glEndList();

}

void drawCircle(double x, double y)
{
	glPushMatrix();

	glTranslatef(x,y,0);

	glCallList(displayID[0]);

	glPopMatrix();
}

void drawQuad(double x, double y)
{
	glPushMatrix();

	glTranslatef(x,y,0);

	glBegin(GL_QUADS);

	glVertex2f(0,0);
	glVertex2f(0,1);
	glVertex2f(1,1);
	glVertex2f(1,0);

	glEnd();

	glPopMatrix();
}

void drawLine(double ax, double ay, double bx, double by)
{
	glPushMatrix();

	glBegin(GL_LINES);

	glVertex2f(ax,ay);
	glVertex2f(bx,by);

	glEnd();

	glPopMatrix();
}

void renderImage()
{
	static int frame=0;

	if (frame==300)
	{
		capture=false;
		return;
	}

	int r=frame/100;
	int s=(frame-r*100)/10;
	int t=frame%10;

	file[7]=r+48;
	file[8]=s+48;
	file[9]=t+48;

	frame++;

	glReadPixels(80, 0, 640, 480,GL_RGB,GL_FLOAT,result);

	for (int i=0;i<640;i++)
	{
		for (int j=0;j<480;j++)
		{
			pRGB[j][i].r=result[3*(j*640+i)]*255;
			pRGB[j][i].g=result[3*(j*640+i)+1]*255;
			pRGB[j][i].b=result[3*(j*640+i)+2]*255;
		}
	}

	Snapshot( ( BYTE*)pRGB, 640, 480, file );
}

void display(void)
{
	if (step==SIMULATE)
	{
		toy->TinkerUpdate();
	}

	ParticleList *pl=&toy->ps->particles;
	ConstraintList *cl=&toy->cs->constraints;

	glClear(GL_COLOR_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();


	glTranslatef(120,100,0);

	glColor3f(0,0,0);

	drawQuad(0,0);	

	for (int i=0;i<pl->size();i++)
	{
		drawQuad(pl->at(i)->pos[0],pl->at(i)->pos[1]*(-1));
	}

	if (onCircle)
	{
		glColor3f(0,0,0);
	}else{
		glColor3f(1,0,0);
	}

	drawCircle(0,0);

	glColor3f(1,0,0);	

	if (drawc)
	{
		drawLine(pos[0],pos[1]*(-1),neg[0],neg[1]*(-1));
	}

	for (int i=0;i<cl->size();i++)
	{
		Particle *a=cl->at(i)->jun;
		Particle *b=cl->at(i)->sen;

		if (a->index<0||b->index<0)
		{
			continue;
		}

		drawLine(a->pos[0],a->pos[1]*(-1),b->pos[0],b->pos[1]*(-1));
	}

	if (capture)
	{
		static int interval=0;

		if (interval==0)
		{
			renderImage();
		}

		interval++;

		if (interval>5)
		{
			interval=0;
		}
		
	}	
	

	glutSwapBuffers();
	glutPostRedisplay();
}

void reshape(int w, int h)
{
	glViewport (0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glOrtho(ORTHO_LEFT, ORTHO_RIGHT, ORTHO_BOTTOM, ORTHO_UP, -1.0, 1.0);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

}

void mouse(int button, int state, int x, int y) 
{
	switch (button)
	{
	case GLUT_LEFT_BUTTON:
		if (state==GLUT_DOWN)
		{
			if (step==ADDPARTICLES)
			{
				if (onCircle)
				{
					toy->AddParticle((x-400)/3.35,(y-265)/3.35,true);
				}else{
					toy->AddParticle((x-400)/3.35,(y-265)/3.35);
				}
				
			}else if (step==ADDCONSTRAINTS)
			{
				start=toy->GetParticle((x-400)/3.35,(y-265)/3.35);

				if (start!=-1)
				{
					pos[0]=toy->ps->particles.at(start)->pos[0];
					pos[1]=toy->ps->particles.at(start)->pos[1];
					addc=true;
				}
				
			}
		}
		if (state==GLUT_UP)
		{
			if (step==ADDCONSTRAINTS&&addc)
			{
				int end=toy->GetParticle((x-400)/3.35,(y-265)/3.35);

				if (end!=-1)
				{
					toy->AddConstraint(start,end);
				}

				addc=false;
				drawc=false;
			}
		}
		break;
	}

	glutPostRedisplay();
}

void motion(int x, int y)
{
	if (addc)
	{
		neg[0]=(x-400)/3.35;
		neg[1]=(y-265)/3.35;
		drawc=true;
	}

	if (step==ADDPARTICLES)
	{
		double dx=(x-400.0)/3.35;
		double dy=(y-265.0)/3.35;

		double dist=sqrt(dx*dx+dy*dy);

		if (abs(dist-RADIUS)<1)
		{
			onCircle=true;
		}else{
			onCircle=false;
		}
	}

	glutPostRedisplay();
}

void keyboard(unsigned char key, int x, int y)
{
	switch (key)
	{
	case 'n':
		step++;
		if (step==SIMULATE)
		{
			for (int i=0;i<toy->ps->particles.size();i++)
			{
				Particle *p=toy->ps->particles.at(i);

				if (p->onCircle)
				{
					toy->AddConstraint(-1,p->index);
				}
			}
			//toy->AddConstraint(-1,0);
			//toy->AddConstraint(-1,toy->ps->particles.size()-1);
			toy->TinkerInit();
		}else{
			glutPassiveMotionFunc(NULL);
			glutMotionFunc(motion);
			onCircle=false;
		}
		break;

	case 'c':
		capture=true;
		break;
	}
}


void testCase()
{
	toy->AddParticle(1,0);
	toy->AddParticle(1,1);
	toy->AddParticle(1,2);
	toy->AddParticle(1,3);
	toy->AddParticle(1,4);
	toy->AddParticle(1,5);
	toy->AddParticle(1,6);
	toy->AddParticle(1,7);
	toy->AddParticle(1,8);
	toy->AddParticle(0,5);
	toy->AddParticle(0,4);
	toy->AddParticle(0,3);
	toy->AddParticle(0,2);
	toy->AddParticle(0,1);
	toy->AddParticle(2,2);
	toy->AddParticle(3,2);
	toy->AddParticle(3,3);

	toy->AddConstraint(-1,0);
	toy->AddConstraint(0,1);
	toy->AddConstraint(1,2);
	toy->AddConstraint(2,3);
	toy->AddConstraint(3,4);
	toy->AddConstraint(4,5);
	toy->AddConstraint(5,6);
	toy->AddConstraint(6,7);
	toy->AddConstraint(7,8);
	toy->AddConstraint(8,9);
	toy->AddConstraint(9,10);
	toy->AddConstraint(10,11);
	toy->AddConstraint(11,12);
	toy->AddConstraint(12,13);
	toy->AddConstraint(-1,13);
	toy->AddConstraint(2,14);
	toy->AddConstraint(14,15);
	toy->AddConstraint(14,16);
	toy->AddConstraint(15,16);

	toy->TinkerInit();
}

int main(int argc, char** argv)
{
	toy=new TinkerToy(0.01,1,1,SINGLEVALUEDECOMPOSITION);

	/*toy->AddParticle(10,0);
	toy->AddParticle(20,0);
	toy->AddParticle(10,0);

	toy->AddConstraint(-1,0);
	toy->AddConstraint(1,0);
	toy->AddConstraint(1,2);
	toy->AddConstraint(-1,2);

	toy->TinkerInit();

	step=SIMULATE;



	toy->TinkerUpdate();
	//toy->TinkerUpdate();

	int s=toy->ls->scale;

	double **temp=toy->ls->A;
	double *ttemp=toy->ls->b;
	double *debug=toy->ls->output;

	for (int i=0;i<s;i++)
	{
		for (int j=0;j<s;j++)
		{
			printf("%.0f ",temp[i][j]);
		}

		//printf("\n");

		printf("   $%f\n",ttemp[i]);
	}

	//toy->TinkerUpdate();

	for (int i=0;i<s;i++)
	{
		printf("%f\n",debug[i]);
	}

	printf("hello\n");*/

	

	glutInit(&argc, argv);
	glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB);
	glutInitWindowSize (WIN_WIDTH, WIN_HEIGHT); 
	glutInitWindowPosition (100, 100);
	glutCreateWindow (argv[0]);
	init ();
	glutDisplayFunc(display); 
	glutReshapeFunc(reshape); 
	glutPassiveMotionFunc(motion);
	glutMouseFunc(mouse);
	glutKeyboardFunc(keyboard);
	glutMainLoop();
	return 0;
}





